import rosbag
from colorama import Fore, Style
import os
import sys
import warnings
import def_function
from datetime import datetime

# 获取当前脚本的完整路径
script_path = os.path.abspath(sys.argv[0])
# 从完整路径中获取目录
script_dir = os.path.dirname(script_path)
# 从完整路径中分离出文件名
script_name = os.path.basename(script_path)
# 使用 splitext() 函数分离文件名和扩展名
script_name_without_extension, extension = os.path.splitext(script_name)

print(Fore.YELLOW+f"python script name:", sys.argv[0])
if len(sys.argv)!=2:
    print(Fore.RED+f"you need python params!")
    sys.exit()

current_rosbag = script_dir + "/" + sys.argv[1]
if os.path.isabs(sys.argv[1]):
    current_rosbag = sys.argv[1]

#print(Fore.WHITE+f"current_rosbag:", current_rosbag)
rosbag_data = rosbag.Bag(current_rosbag)
# 获取类型和主题信息
# rosbag_info = rosbag_data.get_type_and_topic_info()
# info.topics 是一个字典，它的键是 topic 的名字
# all_topics = list(rosbag_info.topics.keys())
topics = ['/vehicle/chassis_report']
error_msg_arry = []

dict_error_info = {
    "brake": def_function.class_data.error_info(),
    "throttle": def_function.class_data.error_info(),
    "steering": def_function.class_data.error_info(),
    "gear": def_function.class_data.error_info(),
    "epb": def_function.class_data.error_info(),
}

brake_report_valid = False
throttle_report_valid = False
steering_report_valid = False
gear_report_valid = False
epb_report_valid = False

for msg_topic, msg, t in rosbag_data.read_messages(topics=topics):
    if msg_topic == '/vehicle/chassis_report':
        brake_report_valid = msg.brake_report.brake_report_data.valid
        throttle_report_valid = msg.throttle_report.throttle_report_data.valid
        steering_report_valid = msg.steering_report.steering_report_data.valid
        gear_report_valid = msg.gear_report.gear_report_data.valid
        epb_report_valid = msg.epb_report.epb_report_data.valid

    if brake_report_valid == False:
        if  dict_error_info['brake'].start_time == 0.0:
            dict_error_info['brake'].start_time = t.to_sec()
            dict_error_info['brake'].message = "刹车故障"
    else:
        dict_error_info['brake'].end_time = t.to_sec()
        if dict_error_info['brake'].start_time != 0.0:
            dict_error_info['brake'].message = "[" + str(dict_error_info['brake'].start_time) + "-" + str(dict_error_info['brake'].end_time) + "]" + dict_error_info['brake'].message
            error_msg_arry.append(dict_error_info['brake'].message)
            dict_error_info['brake'].start_time = 0.0
    
    if throttle_report_valid == False:
        if  dict_error_info['throttle'].start_time == 0.0:
            dict_error_info['throttle'].start_time = t.to_sec()
            dict_error_info['throttle'].message = "油门故障"
    else:
        dict_error_info['throttle'].end_time = t.to_sec()
        if dict_error_info['throttle'].start_time != 0.0:
            dict_error_info['throttle'].message = "[" + str(dict_error_info['throttle'].start_time) + "-" + str(dict_error_info['throttle'].end_time) + "]" + dict_error_info['throttle'].message
            error_msg_arry.append(dict_error_info['throttle'].message)
            dict_error_info['throttle'].start_time = 0.0

    if steering_report_valid == False:
        if  dict_error_info['steering'].start_time == 0.0:
            dict_error_info['steering'].start_time = t.to_sec()
            dict_error_info['steering'].message = "方向盘故障"
    else:
        dict_error_info['steering'].end_time = t.to_sec()
        if dict_error_info['steering'].start_time != 0.0:
            dict_error_info['steering'].message = "[" + str(dict_error_info['steering'].start_time) + "-" + str(dict_error_info['steering'].end_time) + "]" + dict_error_info['steering'].message
            error_msg_arry.append(dict_error_info['steering'].message)
            dict_error_info['steering'].start_time = 0.0
    
    if gear_report_valid == False:
        if  dict_error_info['gear'].start_time == 0.0:
            dict_error_info['gear'].start_time = t.to_sec()
            dict_error_info['gear'].message = "档位故障"
    else:
        dict_error_info['gear'].end_time = t.to_sec()
        if dict_error_info['gear'].start_time != 0.0:
            dict_error_info['gear'].message = "[" + str(dict_error_info['gear'].start_time) + "-" + str(dict_error_info['gear'].end_time) + "]" + dict_error_info['gear'].message
            error_msg_arry.append(dict_error_info['gear'].message)
            dict_error_info['gear'].start_time = 0.0

    if epb_report_valid == False:
        if  dict_error_info['epb'].start_time == 0.0:
            dict_error_info['epb'].start_time = t.to_sec()
            dict_error_info['epb'].message = "电子手刹故障"
    else:
        dict_error_info['epb'].end_time = t.to_sec()
        if dict_error_info['epb'].start_time != 0.0:
            dict_error_info['epb'].message = "[" + str(dict_error_info['epb'].start_time) + "-" + str(dict_error_info['epb'].end_time) + "]" + dict_error_info['epb'].message
            error_msg_arry.append(dict_error_info['epb'].message)
            dict_error_info['epb'].start_time = 0.0

for value in dict_error_info.values():
    if value.start_time != 0.0:
        value.end_time = t.to_sec()
        error_msg_arry.append("[" + str(value.start_time) + "-" + str(value.end_time) + "]" + value.message)

if error_msg_arry:
    print(Fore.RED+f"", script_name_without_extension)
    # 打印数组中的每个元素, 每个元素一行
    for item in error_msg_arry:
        print(Fore.CYAN+f"", item)

rosbag_data.close()
